/**
 ******************************************************************************
 * @file    ax_mpu6050.c
 * @author  Musk Han@XTARK
 * @brief   MPU6050操作源文件 - HAL库版本
 ******************************************************************************
 */

#include "ax_mpu6050.h"

/**
 * 使用方法
void AX_MPU6050_Start() {
    printf("MPU6050 HAL Library Test\r\n");

    // 初始化MPU6050
    AX_MPU6050_Init(&hi2c1);

    // 测试连接
    if (AX_MPU6050_TestConnection() == 0) {
        printf("MPU6050 Connected Successfully!\r\n");
    } else {
        printf("MPU6050 Connection Failed!\r\n");
        while (1);
    }

    // 可选：设置量程和滤波器
    AX_MPU6050_SetAccRange(ACC_RANGE_2G);
    AX_MPU6050_SetGyroRange(GYRO_RANGE_250);
    AX_MPU6050_SetDLPF(DLPF_ACC44_GYRO42);

    while (1) {
        int16_t acc_data[3], gyro_data[3];
        float temperature;

        // 读取传感器数据
        AX_MPU6050_GetAccData(acc_data);
        AX_MPU6050_GetGyroData(gyro_data);
        temperature = AX_MPU6050_GetTempValue();

        // 打印数据
        printf("Acc: X=%6d, Y=%6d, Z=%6d\r\n", acc_data[0], acc_data[1], acc_data[2]);
        printf("Gyro: X=%6d, Y=%6d, Z=%6d\r\n", gyro_data[0], gyro_data[1], gyro_data[2]);
        printf("Temp: %.2f C\r\n\r\n", temperature);

        osDelay(500);
    }
}

 *
 */

// 静态I2C句柄指针
static I2C_HandleTypeDef* hi2c_mpu = NULL;

/**
 * @brief  MPU6050写寄存器
 * @param  reg_address：寄存器地址
 * @param  data：数据内容
 * @retval HAL状态
 */
static HAL_StatusTypeDef MPU6050_WriteRegister(uint8_t reg_address, uint8_t data) {
    if (hi2c_mpu == NULL) return HAL_ERROR;

    return HAL_I2C_Mem_Write(hi2c_mpu, MPU6050_ADDR, reg_address,
                             I2C_MEMADD_SIZE_8BIT, &data, 1, 100);
}

/**
 * @brief  MPU6050读寄存器
 * @param  reg_address：寄存器地址
 * @param  pdata：数据缓冲区指针
 * @param  len：数据长度
 * @retval HAL状态
 */
static HAL_StatusTypeDef MPU6050_ReadRegister(uint8_t reg_address, uint8_t* pdata, uint16_t len) {
    if (hi2c_mpu == NULL) return HAL_ERROR;

    return HAL_I2C_Mem_Read(hi2c_mpu, MPU6050_ADDR, reg_address,
                            I2C_MEMADD_SIZE_8BIT, pdata, len, 100);
}

/**
 * @brief  MPU6050传感器初始化
 * @param  hi2c: I2C句柄指针
 * @retval 无
 */
void AX_MPU6050_Init(I2C_HandleTypeDef* hi2c) {
    hi2c_mpu = hi2c;

    // 等待传感器上电稳定
    HAL_Delay(100);

    // 复位MPU6050
    MPU6050_WriteRegister(MPU6050_PWR_MGMT1_REG, 0x80);  // 复位
    HAL_Delay(100);                                      // 等待复位完成

    // 唤醒MPU6050，使用PLL作为时钟源
    MPU6050_WriteRegister(MPU6050_PWR_MGMT1_REG, 0x01);  // 解除休眠状态，PLL X轴为参考

    // 配置MPU6050寄存器
    MPU6050_WriteRegister(MPU6050_GYRO_CFG_REG, 0x18);   // 陀螺仪量程 ±2000dps
    MPU6050_WriteRegister(MPU6050_ACCEL_CFG_REG, 0x00);  // 加速度量程 ±2g
    MPU6050_WriteRegister(MPU6050_INT_EN_REG, 0x00);     // 关闭中断
    MPU6050_WriteRegister(MPU6050_FIFO_EN_REG, 0x00);    // 关闭FIFO
    MPU6050_WriteRegister(MPU6050_PWR_MGMT2_REG, 0x00);  // 加速度与陀螺仪都工作

    // 设置采样率
    MPU6050_WriteRegister(MPU6050_SAMPLE_RATE_REG, 0x07);  // 1kHz/(7+1)=125Hz
}

/**
 * @brief  MPU6050设置加速度量程
 * @param  range：加速度量程
 *         可设置的加速度量程：ACC_RANGE_2G、ACC_RANGE_4G、ACC_RANGE_8G、ACC_RANGE_16G
 * @retval 无
 */
void AX_MPU6050_SetAccRange(uint8_t range) {
    if (range > 3) range = 0;  // 默认2G
    MPU6050_WriteRegister(MPU6050_ACCEL_CFG_REG, range << 3);
}

/**
 * @brief  MPU6050设置陀螺仪量程
 * @param  range：陀螺仪量程
 *         可设置的陀螺仪量程：GYRO_RANGE_250、GYRO_RANGE_500、GYRO_RANGE_1000、GYRO_RANGE_2000
 * @retval 无
 */
void AX_MPU6050_SetGyroRange(uint8_t range) {
    if (range > 3) range = 0;  // 默认250dps
    MPU6050_WriteRegister(MPU6050_GYRO_CFG_REG, range << 3);
}

/**
 * @brief  MPU6050设置陀螺仪采样率
 * @param  smplrate：陀螺仪采样率，范围4~1000Hz
 * @retval 无
 */
void AX_MPU6050_SetGyroSmplRate(uint16_t smplrate) {
    if (smplrate > 1000) smplrate = 1000;
    if (smplrate < 4) smplrate = 4;

    MPU6050_WriteRegister(MPU6050_SAMPLE_RATE_REG, (uint8_t)(1000 / smplrate - 1));
}

/**
 * @brief  MPU6050设置低通滤波器带宽
 * @param  bandwidth：低通滤波器带宽
 *         可设置的带宽：DLPF_ACC184_GYRO188、DLPF_ACC94_GYRO98、DLPF_ACC44_GYRO42、
 *                      DLPF_ACC21_GYRO20、DLPF_ACC10_GYRO10、DLPF_ACC5_GYRO5
 * @retval 无
 */
void AX_MPU6050_SetDLPF(uint8_t bandwidth) {
    if (bandwidth > 6) bandwidth = 0;  // 关闭DLPF
    MPU6050_WriteRegister(MPU6050_CFG_REG, bandwidth);
}

/**
 * @brief  MPU6050获取传感器温度值
 * @param  无
 * @retval 传感器温度值(摄氏度)
 */
float AX_MPU6050_GetTempValue(void) {
    uint8_t buf[2];
    int16_t raw_temp;

    if (MPU6050_ReadRegister(MPU6050_TEMP_OUTH_REG, buf, 2) != HAL_OK) {
        return 0.0f;
    }

    raw_temp = (int16_t)((buf[0] << 8) | buf[1]);
    return 36.53f + ((float)raw_temp / 340.0f);
}

/**
 * @brief  MPU6050获取三轴加速度寄存器输出值
 * @param  pbuf：读取的数据缓冲区指针(3个int16_t)
 * @retval 无
 */
void AX_MPU6050_GetAccData(int16_t* pbuf) {
    uint8_t buf[6];

    if (MPU6050_ReadRegister(MPU6050_ACCEL_XOUTH_REG, buf, 6) == HAL_OK) {
        pbuf[0] = (int16_t)((buf[0] << 8) | buf[1]);  // X轴
        pbuf[1] = (int16_t)((buf[2] << 8) | buf[3]);  // Y轴
        pbuf[2] = (int16_t)((buf[4] << 8) | buf[5]);  // Z轴
    } else {
        pbuf[0] = pbuf[1] = pbuf[2] = 0;
    }
}

/**
 * @brief  MPU6050获取三轴陀螺仪寄存器输出值
 * @param  pbuf：读取的数据缓冲区指针(3个int16_t)
 * @retval 无
 */
void AX_MPU6050_GetGyroData(int16_t* pbuf) {
    uint8_t buf[6];

    if (MPU6050_ReadRegister(MPU6050_GYRO_XOUTH_REG, buf, 6) == HAL_OK) {
        pbuf[0] = (int16_t)((buf[0] << 8) | buf[1]);  // X轴
        pbuf[1] = (int16_t)((buf[2] << 8) | buf[3]);  // Y轴
        pbuf[2] = (int16_t)((buf[4] << 8) | buf[5]);  // Z轴
    } else {
        pbuf[0] = pbuf[1] = pbuf[2] = 0;
    }
}

/**
 * @brief  测试MPU6050连接
 * @param  无
 * @retval 0: 连接成功, 1: 连接失败
 */
uint8_t AX_MPU6050_TestConnection(void) {
    uint8_t who_am_i = 0;

    if (MPU6050_ReadRegister(MPU6050_WHO_AM_I_REG, &who_am_i, 1) == HAL_OK) {
        return (who_am_i == 0x68) ? 0 : 1;
    }

    return 1;
}